#include "StdAfx.h"
#include "UDPDatagramSocket.h"
#include "NetCVars.h"

#include "Network.h"
#include "Lobby/CryLobby.h"
#if NET_PROFILE_ENABLE
#include "Protocol/PacketRateCalculator.h"
#endif






#if USE_GFWL
#include <winxnet.h>
#endif

#define FRAGMENTED_RESET_ID (0)




#define SOCKET_BUFFER_SIZE	(64*1024)


uint64 g_bytesIn = 0, g_bytesOut = 0;

bool MakeSocketNonBlocking( SOCKET sock )
{
#if defined(WIN32) || defined(XENON) || defined(gringo)
	unsigned long nTrue = 1;
	if (ioctlsocket(sock, FIONBIO, &nTrue) == SOCKET_ERROR)
		return false;




#else
	int nFlags = fcntl(sock, F_GETFL);
	if (nFlags == -1)
		return false;
	nFlags |= O_NONBLOCK;
	if (fcntl(sock, F_SETFL, nFlags) == -1)
		return false;
#endif
	return true;
}

template <class T>
static bool SetSockOpt( SOCKET s, int level, int optname, const T& value )
{
	return 0 == setsockopt( s, level, optname, (const char *)&value, sizeof(T) );
}

union USockAddr
{
	sockaddr_in ip4;
};

#if ENABLE_UDP_PACKET_FRAGMENTATION
static TNetAddress g_nullAddress;
#endif // ENABLE_UDP_PACKET_FRAGMENTATION

CUDPDatagramSocket::CUDPDatagramSocket() : m_socket(INVALID_SOCKET), m_pListener(0)
#if ENABLE_UDP_PACKET_FRAGMENTATION
	, m_pUDPFragBuffer(NULL)
	,	m_pFragmentedPackets(NULL)
	, m_fragmentLossRateAccumulator(0.0f)
	, m_packet_sequence_num(0)
#endif
{
}

CUDPDatagramSocket::~CUDPDatagramSocket()
{
	Cleanup();
	NET_ASSERT( m_socket == INVALID_SOCKET );
	NET_ASSERT( m_sockid == 0 );
#if ENABLE_UDP_PACKET_FRAGMENTATION
	if (m_pUDPFragBuffer)
		delete [] m_pUDPFragBuffer;
	if (m_pFragmentedPackets)
		delete [] m_pFragmentedPackets;
#endif
}

bool CUDPDatagramSocket::Init( SIPv4Addr addr, uint32 flags )
{
	ASSERT_GLOBAL_LOCK;

#if SHOW_FRAGMENTATION_USAGE
	m_fragged=0;
	m_unfragged=0;
#endif//SHOW_FRAGMENTATION_USAGE

#if ENABLE_UDP_PACKET_FRAGMENTATION
	m_RollingIndex=0;
	if (m_pUDPFragBuffer)
		delete [] m_pUDPFragBuffer;
	m_pUDPFragBuffer=new uint8 [FRAG_MAX_MTU_SIZE];
	if (m_pFragmentedPackets)
		delete [] m_pFragmentedPackets;
	m_pFragmentedPackets=new SFragmentedPacket [FRAG_NUM_PACKET_BUFFERS];
	for (uint32 a=0;a<FRAG_NUM_PACKET_BUFFERS;a++)
	{
		ClearFragmentationEntry(a,FRAGMENTED_RESET_ID,g_nullAddress);
	}
#endif//ENABLE_UDP_PACKET_FRAGMENTATION

	m_socket = -1;
	m_bIsIP4 = true;
	m_pSockIO = &CNetwork::Get()->GetSocketIOManager();

	sockaddr_in saddr;
	memset( &saddr, 0, sizeof(saddr) );

	saddr.sin_family = AF_INET;
	saddr.sin_port = htons(addr.port);
	S_ADDR_IP4(saddr) = htonl(addr.addr);

	if (Init( AF_INET, flags, &saddr, sizeof(saddr) ))
	{
#if 0 && defined(WIN32) && !CHECK_ENCODING
		if (!SetSockOpt(m_socket, IPPROTO_IP, IP_DONTFRAGMENT, TRUE))
			return InitWinError();
#endif // WIN32

		m_sockid = m_pSockIO->RegisterSocket( m_socket, m_protocol );
		if (!m_sockid)
		{
			CloseSocket();
			return false;
		}
		m_pSockIO->SetRecvFromTarget( m_sockid, this );
		m_pSockIO->SetSendToTarget( m_sockid, this );
		for (int i=0; i<640; i++)
			m_pSockIO->RequestRecvFrom(m_sockid);

		return true;
	}

	m_socket = -1;
	return false;
}

void CUDPDatagramSocket::Die()
{
	Cleanup();
}

bool CUDPDatagramSocket::IsDead()
{
	return m_socket == INVALID_SOCKET;
}

SOCKET CUDPDatagramSocket::GetSysSocket()
{
    return m_socket;
}

bool CUDPDatagramSocket::Init( int af, uint32 flags, void * pSockAddr, size_t sizeSockAddr )
{
	m_protocol = IPPROTO_UDP;
	int dgram = SOCK_DGRAM;

#if USE_LIVE
	if (gEnv->pNetwork->GetLobby()->GetLobbyServiceType() == eCLS_Online)
	{
		m_protocol = IPPROTO_VDP;
	}
#endif
#if USE_PSN
	if ((gEnv->pNetwork->GetLobby()->GetLobbyServiceType() == eCLS_Online) && (static_cast<sockaddr_in*>(pSockAddr)->sin_port == SCE_NP_PORT))
	{
		m_protocol = IPROTO_UDPP2P_SAFE;
		dgram = SOCK_DGRAM_P2P;
	}
#endif

	m_socket = socket( af, dgram, m_protocol );
	if (m_socket == INVALID_SOCKET)
		return InitWinError();

#if USE_PSN
	if (dgram == SOCK_DGRAM_P2P)
	{
		SetSockOpt (m_socket,SOL_SOCKET,SO_USECRYPTO,TRUE);
	}
#endif

	if (!MakeSocketNonBlocking(m_socket))
		return false;

	enum EFatality
	{
		eF_Fail,
		eF_Log,
		eF_Ignore
	};

	struct SFlag
	{
		int so_level;
		int so_opt;
		ESocketFlags flag;
		int trueVal;
		int falseVal;
		EFatality fatality;
	};

	SFlag allflagsudp[] = 
	{
		{ SOL_SOCKET, SO_BROADCAST, eSF_BroadcastSend, 1, 0, eF_Fail },
		//{ SOL_SOCKET, SO_RCVBUF, eSF_BigBuffer, ((CNetwork::Get()->GetSocketIOManager().caps & eSIOMC_NoBuffering)==0) * 1024*1024, 4096, eF_Ignore },
		{ SOL_SOCKET, SO_RCVBUF, eSF_BigBuffer, SOCKET_BUFFER_SIZE, SOCKET_BUFFER_SIZE, eF_Ignore },
		//{ SOL_SOCKET, SO_SNDBUF, eSF_BigBuffer, ((CNetwork::Get()->GetSocketIOManager().caps & eSIOMC_NoBuffering)==0) * 1024*1024, 4096, eF_Ignore },
		{ SOL_SOCKET, SO_SNDBUF, eSF_BigBuffer, SOCKET_BUFFER_SIZE, SOCKET_BUFFER_SIZE, eF_Ignore },
#if defined(WIN32)
		{ IPPROTO_IP, IP_RECEIVE_BROADCAST, eSF_BroadcastReceive, 1, 0, eF_Ignore },
#endif
	};
	
#if USE_PSN
	SFlag allflagsp2pudp[] = 
	{
		{ SOL_SOCKET, SO_RCVBUF, eSF_BigBuffer, SOCKET_BUFFER_SIZE , SOCKET_BUFFER_SIZE , eF_Ignore },
		{ SOL_SOCKET, SO_SNDBUF, eSF_BigBuffer, SOCKET_BUFFER_SIZE , SOCKET_BUFFER_SIZE , eF_Ignore },
	};
#endif // #if USE_PSN

#if USE_LIVE
	SFlag allflagsvdp[] = 
	{
		{ SOL_SOCKET, SO_RCVBUF, eSF_BigBuffer, SOCKET_BUFFER_SIZE , SOCKET_BUFFER_SIZE , eF_Ignore },
		{ SOL_SOCKET, SO_SNDBUF, eSF_BigBuffer, SOCKET_BUFFER_SIZE , SOCKET_BUFFER_SIZE , eF_Ignore },
	};
#endif

	SFlag* allflags = NULL;
	int numflags = 0;

	switch (m_protocol)
	{
#if USE_PSN
	case IPROTO_UDPP2P_SAFE:
		allflags = allflagsp2pudp;
		numflags = sizeof(allflagsp2pudp)/sizeof(allflagsp2pudp[0]);
		break;
#endif // #if USE_PSN

	case IPPROTO_UDP:
		allflags = allflagsudp;
		numflags = sizeof(allflagsudp)/sizeof(allflagsudp[0]);
		break;

#if USE_LIVE
	case IPPROTO_VDP:
		allflags = allflagsvdp;
		numflags = sizeof(allflagsvdp)/sizeof(allflagsvdp[0]);
		break;
#endif
	}

	for (int i=0; i<numflags; i++)
	{
		if (!SetSockOpt(m_socket, allflags[i].so_level, allflags[i].so_opt, ((flags&allflags[i].flag)==allflags[i].flag)? allflags[i].trueVal : allflags[i].falseVal))
		{
			switch (allflags[i].fatality)
			{
			case eF_Fail:
				return InitWinError();
			case eF_Log:
				LogWinError();
			case eF_Ignore:
				break;
			}
		}
	}

#if USE_PSN
	sockaddr_in_p2p inP2PSock;

	if (m_protocol == IPROTO_UDPP2P_SAFE)
	{
		// Change bind information to be udp2p2 compatible - duplicated to avoid trashing address passed in
		memset(&inP2PSock,0,sizeof(inP2PSock));
		inP2PSock.sin_family = AF_INET;
		inP2PSock.sin_port = htons(SCE_NP_PORT);
		inP2PSock.sin_vport = htons(UDPP2P_VPORT);

		pSockAddr = &inP2PSock;
		sizeSockAddr = sizeof(inP2PSock);
	}
#endif
	if (bind(m_socket, static_cast<sockaddr*>(pSockAddr), sizeSockAddr))
		return InitWinError();

	return true;
}

void CUDPDatagramSocket::Cleanup()
{
	if (m_sockid)
	{
		SCOPED_GLOBAL_LOCK;
		m_pSockIO->UnregisterSocket(m_sockid);
		m_sockid = SSocketID();
	}
	if (m_socket != INVALID_SOCKET)
	{
		CloseSocket();
	}
}

void CUDPDatagramSocket::CloseSocket()
{
	if (m_socket != INVALID_SOCKET)
	{
#if defined(WIN32) || defined(XENON) || defined(gringo) || defined(CAFE)
		closesocket( m_socket );


#else
		close( m_socket );
#endif
		m_socket = INVALID_SOCKET;
	}
}

bool CUDPDatagramSocket::InitWinError()
{
	CloseSocket();
	LogWinError();
	return false;
}

void CUDPDatagramSocket::LogWinError()
{	
#if defined(WIN32) || defined(XENON)
	int error = WSAGetLastError();


#else
	int error = errno;
#endif
	LogWinError( error );
}

void CUDPDatagramSocket::LogWinError( int error )
{
	// ugly
	const char * msg = ((CNetwork*)(gEnv->pNetwork))->EnumerateError( MAKE_NRESULT(NET_FAIL, NET_FACILITY_SOCKET, error) );
	NetWarning( "[net] socket error: %s", msg );
}

void CUDPDatagramSocket::GetSocketAddresses( TNetAddressVec& addrs )
{
	if (m_socket == INVALID_SOCKET)
		return;

#if defined(WIN32) || defined(WIN64) || defined(XENON)
	char addrBuf[_SS_MAXSIZE];
	int addrLen = _SS_MAXSIZE;
	if (0 == getsockname(m_socket, (sockaddr*)addrBuf, &addrLen))
	{
		TNetAddress addr = ConvertAddr((sockaddr*)addrBuf, addrLen);
		bool valid = true;
		if (addr.GetPtr<SNullAddr>())
			valid = false;
		else if (SIPv4Addr * pIPv4 = addr.GetPtr<SIPv4Addr>())
		{
			if (!pIPv4->addr)
				valid = false;
		}
		if (valid)
		{
			addrs.push_back(addr);
			return;
		}
	}
#endif // defined(XENON) || defined(WIN32) || defined(WIN64)

#if !defined(XENON)
	std::vector<string> hostnames;

	uint16 nPort;

	USockAddr sockAddr;
	socklen_t sockAddrSize = sizeof(sockAddr);

	if (0 != getsockname( m_socket, (sockaddr*)&sockAddr, &sockAddrSize ))
	{
		InitWinError();
		return;
	}
	if (sockAddrSize == sizeof(sockaddr_in))
	{
		if (!S_ADDR_IP4(sockAddr.ip4))
			hostnames.push_back("localhost");
		nPort = ntohs( sockAddr.ip4.sin_port );
	}
	else
	{
#ifdef _DEBUG
		CryFatalError( "Unhandled sockaddr type" );
#endif
		return;
	}

#if !defined(PS3) // FIXME ?
	char hostnameBuffer[NI_MAXHOST];
	if (!gethostname(hostnameBuffer, sizeof(hostnameBuffer)))
		hostnames.push_back( hostnameBuffer );
#endif

	if (hostnames.empty())
		return;

	for (std::vector<string>::const_iterator iter = hostnames.begin(); iter != hostnames.end(); ++iter)
	{
		hostent * hp = gethostbyname( iter->c_str() );
		if (hp)
		{
			switch (hp->h_addrtype)
			{
			case AF_INET:
				{
					SIPv4Addr addr;
					NET_ASSERT( sizeof(addr.addr) == hp->h_length );
					addr.port = nPort;
					for (size_t i=0; hp->h_addr_list[i]; i++)
					{
						addr.addr = ntohl( *(uint32*)hp->h_addr_list[i] );
						addrs.push_back( TNetAddress(addr) );
					}
				}
				break;
			default:
				NetWarning("Unhandled network address type %d length %d bytes", hp->h_addrtype, hp->h_length);
			}
		}
	}
#endif // defined(XENON)
}

void CUDPDatagramSocket::RegisterBackoffAddress( TNetAddress addr )
{
	m_pSockIO->RegisterBackoffAddressForSocket( addr, m_sockid );
}

void CUDPDatagramSocket::UnregisterBackoffAddress( TNetAddress addr )
{
	m_pSockIO->UnregisterBackoffAddressForSocket( addr, m_sockid );
}

ESocketError CUDPDatagramSocket::Send( const uint8 * pBuffer, size_t nLength, const TNetAddress& to )
{
#if ENABLE_DEBUG_KIT
	CAutoCorruptAndRestore acr(pBuffer, nLength, CVARS.RandomPacketCorruption == 1);
#endif
	

#if ENABLE_UDP_PACKET_FRAGMENTATION
	if (nLength > FRAG_MAX_MTU_SIZE)
	{
		if (nLength > ((FRAG_MAX_MTU_SIZE-fho_FragHeaderSize)*FRAG_MAX_FRAGMENTS))
		{
#if SHOW_FRAGMENTATION_USAGE
			NetQuickLog(true, 10.f, "[Fragmentation]: Packet too big for fragmentation buffer, Packet Size = %d", static_cast<int>(nLength));
#endif // SHOW_FRAGMENTATION_USAGE
			return eSE_BufferTooSmall;
		}

		SendFragmented(pBuffer,nLength,to);
	}
	else
#endif // ENABLE_UDP_PACKET_FRAGMENTATION
	{
#if SHOW_FRAGMENTATION_USAGE
		m_unfragged++;
#endif

		g_bytesOut += nLength + UDP_HEADER_SIZE;
		if (g_time > CNetCVars::Get().StallEndTime)
			m_pSockIO->RequestSendTo( m_sockid, to, pBuffer, nLength );
	}
	
	return eSE_Ok;
}

ESocketError CUDPDatagramSocket::SendVoice( const uint8 * pBuffer, size_t nLength, const TNetAddress& to )
{
#if ENABLE_DEBUG_KIT
	CAutoCorruptAndRestore acr(pBuffer, nLength, CVARS.RandomPacketCorruption == 1);
#endif

	g_bytesOut += nLength + UDP_HEADER_SIZE;
	if (g_time > CNetCVars::Get().StallEndTime)
		m_pSockIO->RequestSendVoiceTo( m_sockid, to, pBuffer, nLength );

	return eSE_Ok;
}

void CUDPDatagramSocket::OnRecvFromComplete( const TNetAddress& from, const uint8 * pData, uint32 len )
{
	g_bytesIn += len + UDP_HEADER_SIZE;

#if NET_MINI_PROFILE
	g_socketBandwidth.totalBandwidthRecvd += (len + UDP_HEADER_SIZE) * 8;
#endif
#if NET_PROFILE_ENABLE
	g_socketBandwidth.totalBandwidthRecvd += (len + UDP_HEADER_SIZE) * 8;
	g_socketBandwidth.sizeRecv += 8 * (len + UDP_HEADER_SIZE);
#endif

#if ENABLE_UDP_PACKET_FRAGMENTATION

	pData = ReceiveFragmented(from,pData,len);		// No point checking for fragmented packets if support for them is off
	if (pData == NULL)
	{
		m_pSockIO->RequestRecvFrom(m_sockid);				// prevent drain of recv counter
		return;																			// early out, fragmented packet is not complete
	}

#endif

	if (m_pListener)
	{
		m_pListener->OnPacket( from, pData, len );
	}

	m_pSockIO->RequestRecvFrom(m_sockid);
}

void CUDPDatagramSocket::OnRecvFromException( const TNetAddress& from, ESocketError err )
{
	if (err != eSE_Cancelled)
	{
		if (m_pListener)
			m_pListener->OnError( from, err );

		m_pSockIO->RequestRecvFrom(m_sockid);
	}
}

void CUDPDatagramSocket::OnSendToException( const TNetAddress& from, ESocketError err )
{
	if (err != eSE_Cancelled)
	{
		if (m_pListener)
			m_pListener->OnError( from, err );
	}
}

void CUDPDatagramSocket::SetListener( IDatagramListener * pListener )
{
	m_pListener = pListener;
}

#if ENABLE_UDP_PACKET_FRAGMENTATION
/*static void DumpBytes( const uint8 * p, size_t len )
{
	char l[256];

	char *o = l;
	for (size_t i=0; i<len; i++)
	{
		o += sprintf(o, "%.2x ", p[i]);
		if ((i & 31)==31)
		{
			NetLog(l);
			o = l;
		}
	}
	if (len & 31)
		NetLog(l);
}*/

void CUDPDatagramSocket::ClearFragmentationEntry(uint32 index,TFragPacketId id,const TNetAddress &from)
{
	assert(index<FRAG_NUM_PACKET_BUFFERS);
	m_pFragmentedPackets[index].m_from=from;
	m_pFragmentedPackets[index].m_Id=id;
	m_pFragmentedPackets[index].m_ReconstitutionMask=0;
	m_pFragmentedPackets[index].m_Length=0;
	m_pFragmentedPackets[index].m_inUse = (from.GetPtr<SNullAddr>()) ? false : true;
	m_pFragmentedPackets[index].m_sequence_num = (from.GetPtr<SNullAddr>()) ? 0 : ++m_packet_sequence_num;
}

void CUDPDatagramSocket::SendFragmented(const uint8 * pBuffer, size_t nLength, const TNetAddress& to)
{
	TFragSeqTransport		fraggedCnt=0;
	TFragSeqTransport		expectedFragmentedPacketCount=(nLength + ((FRAG_MAX_MTU_SIZE-fho_FragHeaderSize)-1)) / 
																							(FRAG_MAX_MTU_SIZE-fho_FragHeaderSize);

	assert(expectedFragmentedPacketCount<=FRAG_MAX_FRAGMENTS);
	assert(expectedFragmentedPacketCount>1);

	m_RollingIndex++; // Always increase the rolling index when sending a fragmented packet, helps to avoid clashes
#if UDP_PACKET_FRAGMENTATION_CRC_CHECK
	uint crc = gEnv->pSystem->GetCrc32Gen()->GetCRC32((const char*)pBuffer, nLength, 0xffffffff);
#endif

#if SHOW_FRAGMENTATION_USAGE
	m_fragged++;
#if UDP_PACKET_FRAGMENTATION_CRC_CHECK
	NetQuickLog(true, 10.f, "[Fragmentation]: Sending fragmented packet %d (fragged %d / unFragged %d), size = %d, crc %08X, to %s", m_RollingIndex, m_fragged, m_unfragged, nLength, crc, RESOLVER.ToString(to).c_str());
#else
	NetQuickLog(true, 10.f, "[Fragmentation]: Sending fragmented packet %d (fragged %d / unFragged %d), size = %d, to %s", m_RollingIndex, m_fragged, m_unfragged, nLength, RESOLVER.ToString(to).c_str());
#endif // UDP_PACKET_FRAGMENTATION_CRC_CHECK
#endif // SHOW_FRAGMENTATION_USAGE

	while (nLength)
	{
		int blkSize = MIN(nLength,FRAG_MAX_MTU_SIZE-fho_FragHeaderSize);

		m_pUDPFragBuffer[fho_HeaderID] = Frame_IDToHeader[eH_Fragmentation];
		memcpy(&m_pUDPFragBuffer[fho_PacketID], &m_RollingIndex, sizeof(m_RollingIndex));
		TFragSeqTransport* pFragInfo = &m_pUDPFragBuffer[fho_FragInfo];
		*pFragInfo = fraggedCnt<<FRAG_SEQ_BIT_SIZE;
		*pFragInfo |= expectedFragmentedPacketCount - 1;
		fraggedCnt++;
#if UDP_PACKET_FRAGMENTATION_CRC_CHECK
		memcpy(&m_pUDPFragBuffer[fho_Checksum], &crc, sizeof(crc));
#endif
		memcpy(&m_pUDPFragBuffer[fho_FragHeaderSize],pBuffer,blkSize);

		nLength-=blkSize;
		pBuffer+=blkSize;

		//DumpBytes(m_pUDPFragBuffer, blkSize + fho_FragHeaderSize);

		g_bytesOut += blkSize + UDP_HEADER_SIZE;
		if (g_time > CNetCVars::Get().StallEndTime)
		{
			m_fragmentLossRateAccumulator += CVARS.PacketFragmentLossRate;
			if (m_fragmentLossRateAccumulator >= 1.0f)
			{
				m_fragmentLossRateAccumulator -= 1.0f;
			}
			else
			{
				m_pSockIO->RequestSendTo( m_sockid, to, m_pUDPFragBuffer, blkSize+fho_FragHeaderSize);
			}
		}
	}
}

const uint8 *CUDPDatagramSocket::ReceiveFragmented(const TNetAddress& from, const uint8 * pData, uint32 &len)
{
	uint8 nType = Frame_HeaderToID[pData[fho_HeaderID]];
	if (nType==eH_Fragmentation)
	{
		TFragPacketId fragmentedPacketID;
		memcpy(&fragmentedPacketID, &pData[fho_PacketID], sizeof(fragmentedPacketID));
		int bufferIndex = FindBufferIndex(from, fragmentedPacketID);
		if (bufferIndex == FRAG_NUM_PACKET_BUFFERS)
		{
#if SHOW_FRAGMENTATION_USAGE
			NetQuickLog(true, 10.f, "[Fragmentation]: Unable to find free buffer for id %d from '%s'", fragmentedPacketID, RESOLVER.ToString(from).c_str());
#endif // SHOW_FRAGMENTATION_USAGE
			return NULL;
		}

		const TFragSeqTransport* pFragInfo = &pData[fho_FragInfo];
		uint8 fragmentIndex = ((*pFragInfo)>>FRAG_SEQ_BIT_SIZE);
		TFragSeqStorage expected = ~(~(TFragSeqStorage(0))<<(((*pFragInfo)&((1<<FRAG_SEQ_BIT_SIZE)-1))+1));
		TFragSeqStorage sequence = TFragSeqStorage(1) << fragmentIndex;
		uint8 rSeq=fragmentIndex;

		if(m_pFragmentedPackets[bufferIndex].m_ReconstitutionMask & sequence)
		{
#if SHOW_FRAGMENTATION_USAGE
			NetQuickLog(true, 10.f, "[Fragmentation]: Received already reconstructed UPD fragment from %s, discarding old buffer", RESOLVER.ToString(from).c_str());
#endif // SHOW_FRAGMENTATION_USAGE
			ClearFragmentationEntry(bufferIndex, fragmentedPacketID, from);
		}

		m_pFragmentedPackets[bufferIndex].m_lastUpdate = g_time;
		m_pFragmentedPackets[bufferIndex].m_ReconstitutionMask |= sequence;
#if SHOW_FRAGMENTATION_USAGE_VERBOSE
		NetQuickLog(true, 10.f, "[Fragmentation]: received fragment %d, rseq %d, now have %x (expecting %x)", fragmentIndex+1, rSeq, m_pFragmentedPackets[bufferIndex].m_ReconstitutionMask, expected);
#endif // SHOW_FRAGMENTATION_USAGE_VERBOSE

		memcpy(&m_pFragmentedPackets[bufferIndex].m_FragPackets[0+rSeq*(FRAG_MAX_MTU_SIZE-fho_FragHeaderSize)],
					&pData[fho_FragHeaderSize],len-fho_FragHeaderSize);
		
#if UDP_PACKET_FRAGMENTATION_CRC_CHECK
		uint crcOrigin;
		memcpy(&crcOrigin, &pData[fho_Checksum], sizeof(crcOrigin));
#endif
		assert(len-fho_FragHeaderSize>0);
		
		m_pFragmentedPackets[bufferIndex].m_Length+=len-fho_FragHeaderSize;

		//DumpBytes(pData, len);

		if (m_pFragmentedPackets[bufferIndex].m_ReconstitutionMask == expected)
		{
			// Complete packet received... 
			pData = &m_pFragmentedPackets[bufferIndex].m_FragPackets[0];
			len = m_pFragmentedPackets[bufferIndex].m_Length;
#if SHOW_FRAGMENTATION_USAGE
			NetQuickLog(true, 10.f, "[Fragmentation]: buffer %d COMPLETE for packet %d", bufferIndex, m_pFragmentedPackets[bufferIndex].m_Id);
#endif

#if UDP_PACKET_FRAGMENTATION_CRC_CHECK
			uint crcRecv = gEnv->pSystem->GetCrc32Gen()->GetCRC32((const char*)pData, len, 0xffffffff);
			if (crcRecv != crcOrigin)
			{
				NetQuickLog(true, 10.f, "[Fragmentation]: Original checksum %u differs from that received %u for buffer %d", crcOrigin, crcRecv, bufferIndex);
			}
#endif

			ClearFragmentationEntry(bufferIndex,FRAGMENTED_RESET_ID,g_nullAddress);			// fix for potential corrupted packets due to not clearing buffer states.
		}
		else
		{
			return NULL;
		}
	}

	return pData;
}

void CUDPDatagramSocket::DiscardStaleEntries()
{
	TNetAddress sources[FRAG_NUM_PACKET_BUFFERS]; // unique sources
	int sources_occupied[FRAG_NUM_PACKET_BUFFERS]; // number of packets per source
	
	uint sources_oldest_packets_seq[FRAG_NUM_PACKET_BUFFERS]; // smallest packet id per source
	int sources_oldest_packets_index[FRAG_NUM_PACKET_BUFFERS]; // smallest packet id per source

	// collect unique sources
	int last_source = 0;
	for(int i = 0; i != FRAG_NUM_PACKET_BUFFERS; ++i)
	{
		SFragmentedPacket& packet = m_pFragmentedPackets[i];
		if(!packet.m_inUse)
			continue;

		if (g_time.GetDifferenceInSeconds(packet.m_lastUpdate) > CNetCVars::Get().net_fragment_expiration_time)
		{
#if SHOW_FRAGMENTATION_USAGE
			NetQuickLog(true, 10.f, "[Fragmentation]: reconstruction buffer %d from %s is stale (time) - REUSING", i, RESOLVER.ToString(packet.m_from).c_str());
#endif // SHOW_FRAGMENTATION_USAGE
			ClearFragmentationEntry(i, FRAGMENTED_RESET_ID, g_nullAddress);
			continue;
		}

		int s;
		for(s = 0; s != last_source; ++s)
		{
			if(sources[s] == packet.m_from)
			{
				++sources_occupied[s];
				if(packet.m_sequence_num < sources_oldest_packets_seq[s])
				{
					sources_oldest_packets_seq[s] = packet.m_sequence_num;
					sources_oldest_packets_index[s] = i;
				}
				break;
			}
		}

		if(s == last_source)
		{
			sources[last_source] = packet.m_from;
			sources_occupied[last_source] = 1;
			sources_oldest_packets_seq[last_source] = packet.m_sequence_num;
			sources_oldest_packets_index[last_source] = i;
			++last_source;
		}
	}

	// discard packets
	for(int s = 0; s != last_source; ++s)
	{
		if(sources_occupied[s] <= CNetCVars::Get().net_max_fragmented_packets_per_source)
			continue;

		int idx = sources_oldest_packets_index[s];
		SFragmentedPacket& packet = m_pFragmentedPackets[idx];

#if SHOW_FRAGMENTATION_USAGE
		NetQuickLog(true, 10.f, "[Fragmentation]: Dropping stale packet assembly buffer %d from %s, packet id %u", idx, RESOLVER.ToString(packet.m_from).c_str(), packet.m_Id);
#endif // SHOW_FRAGMENTATION_USAGE

		ClearFragmentationEntry(idx, FRAGMENTED_RESET_ID, g_nullAddress);
	}
}

uint8 CUDPDatagramSocket::FindBufferIndex(const TNetAddress& from, TFragPacketId id)
{
	DiscardStaleEntries();

	int match = FRAG_NUM_PACKET_BUFFERS, unused = FRAG_NUM_PACKET_BUFFERS;

	for(int i = 0; i != FRAG_NUM_PACKET_BUFFERS; ++i)
	{
		SFragmentedPacket& packet = m_pFragmentedPackets[i];
		if(!packet.m_inUse)
		{
			unused = i;
			continue;
		}

		if((packet.m_from == from) && (packet.m_Id == id))
		{
			match = i;
			break;
		}
	}

	if(match != FRAG_NUM_PACKET_BUFFERS)
	{
#if SHOW_FRAGMENTATION_USAGE_VERBOSE
		NetQuickLog(true, 10.f, "[Fragmentation]: using EXISTING buffer %d for packet %d from %s", match, id, RESOLVER.ToString(from).c_str());
#endif // SHOW_FRAGMENTATION_USAGE_VERBOSE
		return match;
	}
	else if(unused != FRAG_NUM_PACKET_BUFFERS)
	{
#if SHOW_FRAGMENTATION_USAGE_VERBOSE
		NetQuickLog(true, 10.f, "[Fragmentation]: using NEW buffer %d for packet %d from %s", unused, id, RESOLVER.ToString(from).c_str());
#endif // SHOW_FRAGMENTATION_USAGE_VERBOSE
		ClearFragmentationEntry(unused, id, from);
		return unused;
	}
	
	return FRAG_NUM_PACKET_BUFFERS;
}
#endif
